// Ardustation REBORN!
// This code is highly alpha quality, be aware...
#include <GCS_MAVLink.h>
#include <avr/pgmspace.h>
#include <LiquidCrystal.h>
#include <EEPROM.h>
#include "parameter.h"


#define SERIAL_BAUD 57600
//#define BUZZERON // is the buzzer on?

// data streams active and rates
#define MAV_DATA_STREAM_POSITION_ACTIVE 1
#define MAV_DATA_STREAM_RAW_SENSORS_ACTIVE 1
#define MAV_DATA_STREAM_EXTENDED_STATUS_ACTIVE 1
#define MAV_DATA_STREAM_RC_CHANNELS_ACTIVE 1
#define MAV_DATA_STREAM_RAW_CONTROLLER_ACTIVE 0
#define MAV_DATA_STREAM_EXTRA1_ACTIVE 1

// update rate is times per second (hz)
#define MAV_DATA_STREAM_POSITION_RATE 5
#define MAV_DATA_STREAM_RAW_SENSORS_RATE 5
#define MAV_DATA_STREAM_EXTENDED_STATUS_RATE 5
#define MAV_DATA_STREAM_RAW_CONTROLLER_RATE 5
#define MAV_DATA_STREAM_RC_CHANNELS_RATE 5
#define MAV_DATA_STREAM_EXTRA1_RATE 5

#define GET_PARAMS_TIMEOUT 200 //(20 seconds)

// store strings in flash memory
#define MAIN_MENU 0
#define START_FEEDS 1
#define STOP_FEEDS 2
#define FLIGHT_DATA 3
#define PIDS 4
#define GET_PARAMS 5
#define EDIT_VIEW_PARAMS 6
#define INIT_EEPROM 7
#define MENU8 8
#define EDIT_PARAM 10
#define SAVE_PARAM 11

#define TOTAL_PARAMS 45

LiquidCrystal lcd(2, 3, 4, 5, 6, 7, 8); //Initailizing LCD object (this is real C++).

void lcd_print_P(const char *string)
{
  char c;
  while( (c = pgm_read_byte(string++)) )
    lcd.write(c);
}

#define toRad(x) (x*PI)/180.0
#define toDeg(x) (x*180.0)/PI

unsigned long hb_timer;
unsigned long timer;

// flight data
int altitude=0;
float pitch=0;
float roll=0;
float yaw=0;
float longitude=0;
float latitude=0;
float velocity = 0;
byte numSats=0;
float battery=0;
byte currentSMode=0;
byte currentNMode=0;
int scroll = 0;

// rc channels
int16_t chan1 = 0;
int16_t chan2 = 0;
int16_t chan3 = 0;
int16_t chan4 = 0;
int16_t chan5 = 0;
int16_t chan6 = 0;
int16_t chan7 = 0;
int16_t chan8 = 0;

// menus etc
byte menu=0;
byte subMenu=0;
byte menuMode=0;
byte currentOption=0;
byte lastOption=0;
byte timeOut=0;
byte redraw=1;
byte waitingAck=0;
byte paramsRecv=0;
byte beat=0;
parameter editParm;

byte heart[8] = {
  B00000,
  B01010,
  B11111,
  B11111,
  B01110,
  B00100,
  B00000,
  B00000,
};

byte upArrow[8] = {
  B00000,
  B00100,
  B01010,
  B10001,
  B00000,
  B00000,
  B00000,
  B00000,
};

byte downArrow[8] = {
  B00000,
  B00000,
  B00000,
  B00000,
  B10001,
  B01010,
  B00100,
  B00000,
};

void EEPROM_writeRevision()
{
  EEPROM.write(0,73);
  EEPROM.write(1,37);
  EEPROM.write(2,75);
/*  byte lowByte = ((paramCount >> 0) & 0xFF);
  byte highByte = ((paramCount >> 8) & 0xFF);
  EEPROM.write(3, lowByte);
  EEPROM.write(4, highByte);*/
}

int EEPROM_readRevision()
{
  if (EEPROM.read(0) == 73 && EEPROM.read(1) == 37 && EEPROM.read(2) == 75)
  {
    byte lowByte = EEPROM.read(3);
    byte highByte = EEPROM.read(4);
    return (0);
  }
  else
    return -1;
}

int EEPROM_writeParameter(int ee, parameter& value)
{
    ee += 3; //account for revision info
    const byte* p = (const byte*)(const void*)&value;
    int i;
    for (i = 0; i < sizeof(value); i++)
    {
	  EEPROM.write(ee++, *p++);
    }
    return i;
}

int EEPROM_writeFloat(int ee, float& value)
{
    ee += 3; //account for revision info
    const byte* p = (const byte*)(const void*)&value;
    int i;
    for (i = 0; i < sizeof(value); i++)
    {
	  EEPROM.write(ee++, *p++);
    }
    return i;
}

int EEPROM_readParameter(int ee, parameter& value)
{
    ee += 3; //account for revision info
    byte* p = (byte*)(void*)&value;
    int i;
    for (i = 0; i < sizeof(value); i++)
	  *p++ = EEPROM.read(ee++);
    return i;
}

// parameters
prog_char param_0[] PROGMEM = "ACR_RLL_P";
prog_char param_1[] PROGMEM = "ACR_RLL_I";
prog_char param_2[] PROGMEM = "ACR_RLL_D";
prog_char param_3[] PROGMEM = "ACR_RLL_IMAX";
prog_char param_4[] PROGMEM = "ACR_PIT_P";
prog_char param_5[] PROGMEM = "ACR_PIT_I";
prog_char param_6[] PROGMEM = "ACR_PIT_D";
prog_char param_7[] PROGMEM = "ACR_PIT_IMAX";
prog_char param_8[] PROGMEM = "ACR_YAW_P";
prog_char param_9[] PROGMEM = "ACR_YAW_I";
prog_char param_10[] PROGMEM = "ACR_YAW_D";
prog_char param_11[] PROGMEM = "ACR_YAW_IMAX";
prog_char param_12[] PROGMEM = "STB_RLL_P";
prog_char param_13[] PROGMEM = "STB_RLL_I";
prog_char param_14[] PROGMEM = "STB_RLL_D";
prog_char param_15[] PROGMEM = "STB_RLL_IMAX";
prog_char param_16[] PROGMEM = "STB_PIT_P";
prog_char param_17[] PROGMEM = "STB_PIT_I";
prog_char param_18[] PROGMEM = "STB_PIT_D";
prog_char param_19[] PROGMEM = "STB_PIT_IMAX";
prog_char param_20[] PROGMEM = "STB_YAW_P";
prog_char param_21[] PROGMEM = "STB_YAW_I";
prog_char param_22[] PROGMEM = "STB_YAW_D";
prog_char param_23[] PROGMEM = "STB_YAW_IMAX";
prog_char param_24[] PROGMEM = "NAV_LAT_P";
prog_char param_25[] PROGMEM = "NAV_LAT_I";
prog_char param_26[] PROGMEM = "NAV_LAT_D";
prog_char param_27[] PROGMEM = "NAV_LAT_IMAX";
prog_char param_28[] PROGMEM = "NAV_LON_P";
prog_char param_29[] PROGMEM = "NAV_LON_I";
prog_char param_30[] PROGMEM = "NAV_LON_D";
prog_char param_31[] PROGMEM = "NAV_LON_IMAX";
prog_char param_32[] PROGMEM = "NAV_WP_P";
prog_char param_33[] PROGMEM = "NAV_WP_I";
prog_char param_34[] PROGMEM = "NAV_WP_D";
prog_char param_35[] PROGMEM = "NAV_WP_IMAX";
prog_char param_36[] PROGMEM = "THR_BAR_P";
prog_char param_37[] PROGMEM = "THR_BAR_I";
prog_char param_38[] PROGMEM = "THR_BAR_D";
prog_char param_39[] PROGMEM = "THR_BAR_IMAX";
prog_char param_40[] PROGMEM = "THR_SON_P";
prog_char param_41[] PROGMEM = "THR_SON_I";
prog_char param_42[] PROGMEM = "THR_SON_D";
prog_char param_43[] PROGMEM = "THR_SON_IMAX";
prog_char param_44[] PROGMEM = "MAG_ENABLE";

PROGMEM const char *paramTable[] = {
  param_0, param_1, param_2, param_3, param_4, param_5, param_6, param_7, param_8, param_9,
  param_10, param_11, param_12, param_13, param_14, param_15, param_16, param_17, param_18, param_19,
  param_20, param_21, param_22, param_23, param_24, param_25, param_26, param_27, param_28, param_29,
  param_30, param_31, param_32, param_33, param_34, param_35, param_36, param_37, param_38, param_39,
  param_40, param_41, param_42, param_43, param_44 };

int get_Param_Key(char *buffer, int index)
{
  if (index < TOTAL_PARAMS)
  {
    strcpy_P(buffer, (char*)pgm_read_word(&(paramTable[index])));
    return 0;
  }
  else
    return -1;
}

void init_EEPROM()
{
  lcd.clear();
  lcd_print_P(PSTR("Init EEPROM..."));
  char buffer[15];
  for (int i=0; i<TOTAL_PARAMS; i++)
  {
    parameter temp;
    temp.value = 0;
    get_Param_Key(temp.key, i);
    EEPROM_writeParameter(i * sizeof(temp), temp);
  }
  //EEPROM_writeParamCount(TOTAL_PARAMS);
  delay(1000);
  menu = MAIN_MENU;
  redraw = 1;
}

int find_param(const char* key)
{
  char buffer[15];
  for (int i=0; i<TOTAL_PARAMS; i++)
  {
    get_Param_Key(buffer, i);
    if (strcmp(buffer,(const char*)key) == 0)
      return i;    
  }
  return -1;
}

void gcs_update()
{
  // receive new packets
  mavlink_message_t msg;
  mavlink_status_t status;

  // process received bytes
  while(Serial.available())
  {
      uint8_t c = Serial.read();
      // Try to get a new message
      if(mavlink_parse_char(0, c, &msg, &status)) gcs_handleMessage(&msg);
  }
}

void gcs_handleMessageAttitude(mavlink_message_t* msg)
{
  mavlink_attitude_t packet;
  mavlink_msg_attitude_decode(msg, &packet);
  pitch = toDeg(packet.pitch);
  yaw = toDeg(packet.yaw);
  roll = toDeg(packet.roll);
  return;
}

void gcs_handleMessageGpsRaw(mavlink_message_t* msg)
{
  mavlink_gps_raw_t packet;
  mavlink_msg_gps_raw_decode(msg, &packet);
  latitude = packet.lat;
  longitude = packet.lon;
  velocity = packet.v;
  altitude = packet.alt;
  return;
}

void gcs_handleMessageGpsStatus(mavlink_message_t* msg)
{
  mavlink_gps_status_t packet;
  mavlink_msg_gps_status_decode(msg, &packet);        
  numSats = packet.satellites_visible;
  return;
}

void gcs_handleMessageRawPressure(mavlink_message_t* msg)
{
  mavlink_raw_pressure_t packet;
  mavlink_msg_raw_pressure_decode(msg, &packet);
  return;  
}

void gcs_handleMessageRcChannelsRaw(mavlink_message_t* msg)
{
  mavlink_rc_channels_raw_t packet;
  mavlink_msg_rc_channels_raw_decode(msg, &packet);
  chan1 = packet.chan1_raw;
  chan2 = packet.chan2_raw;
  chan3 = packet.chan3_raw;
  chan4 = packet.chan4_raw;
  chan5 = packet.chan5_raw;
  chan6 = packet.chan6_raw;
  chan7 = packet.chan7_raw;
  chan8 = packet.chan8_raw;
  return; 
}

void gcs_handleMessageSysStatus(mavlink_message_t* msg)
{
  mavlink_sys_status_t packet;
  mavlink_msg_sys_status_decode(msg, &packet);
  currentSMode = packet.mode;
  currentNMode = packet.nav_mode;
  battery = packet.vbat;
  return;  
}

void gcs_handleMessageParamValue(mavlink_message_t* msg)
{
  mavlink_param_value_t packet;
  mavlink_msg_param_value_decode(msg, &packet);
  const char * key = (const char*) packet.param_id;
  int loc = find_param(key);
  if (loc != -1)
  {
    float value;
    value = packet.param_value;
    EEPROM_writeFloat((loc * sizeof(editParm))+sizeof(editParm.key), value);
    paramsRecv++;
  }
  if (waitingAck == 1)
  {        
    if (strcmp(key, editParm.key) == 0)
    {
      waitingAck = 0;
    }
  }
  //else
  //  timeOut = 100; // each time we get another parameter reset the timeout
  //redraw = 1;
  return;
}

void gcs_handleMessage(mavlink_message_t* msg)
{
  switch (msg->msgid) {
    case MAVLINK_MSG_ID_HEARTBEAT:
    {
      beat = 1;
      break;
    }
   case MAVLINK_MSG_ID_ATTITUDE:
    {
      gcs_handleMessageAttitude(msg);
      break;
    }
    case MAVLINK_MSG_ID_GPS_RAW:
    {
      gcs_handleMessageGpsRaw(msg);
      break;
    }
    case MAVLINK_MSG_ID_GPS_STATUS:
    {
      gcs_handleMessageGpsStatus(msg);
      break;
    }
    case MAVLINK_MSG_ID_RAW_PRESSURE:
    {
      gcs_handleMessageRawPressure(msg);
      break;
    }
    case MAVLINK_MSG_ID_RC_CHANNELS_RAW:
    {
      gcs_handleMessageRcChannelsRaw(msg);
      break;
    }
    case MAVLINK_MSG_ID_SYS_STATUS:
    {
      gcs_handleMessageSysStatus(msg);
      break;
    }
    case MAVLINK_MSG_ID_PARAM_VALUE:
    {
      gcs_handleMessageParamValue(msg);
      break;
    }
  }
}

void send_message(mavlink_message_t* msg)
{
  Serial.write(MAVLINK_STX);
  Serial.write(msg->len);
  Serial.write(msg->seq);
  Serial.write(msg->sysid);
  Serial.write(msg->compid);
  Serial.write(msg->msgid);
  for(uint16_t i = 0; i < msg->len; i++)
  {
    Serial.write(msg->payload[i]);
  }
  Serial.write(msg->ck_a);
  Serial.write(msg->ck_b);
}

void setup()
{
  Serial.begin(SERIAL_BAUD);
  lcd.createChar(0, heart);
  lcd.createChar(1, downArrow);
  lcd.createChar(2, upArrow);
  lcd.begin(20, 4);
  pinMode(14,OUTPUT); //Pin mode as output to control buzzer (analog0)
  pinMode(11,OUTPUT);
  lcd.clear();
  lcd_print_P(PSTR("   ArduStationPS"));
  lcd.setCursor(0, 1);
  lcd_print_P(PSTR("   with MAVLink"));
  lcd.setCursor(0, 2);
  lcd_print_P(PSTR(" updated by psmitty"));
  delay(2000);
  if (EEPROM_readRevision() != 0)
  {
    init_EEPROM();
    EEPROM_writeRevision();
  }  
  lcd.clear();
  lcd_print_P(PSTR("Starting xbee..."));
  delay(2000);
}

void loop()
{
  switch (menu)
  {
    case MAIN_MENU: // main menu
    {
      if (redraw == 1)
        main_menu();
      break;
    }
    case START_FEEDS: // start feeds
    {
      start_feeds();
      break;
    }
    case STOP_FEEDS: // stop feeds
    {
      stop_feeds();
      break;
    }  
    case FLIGHT_DATA: // flight data
    {
      flight_data();
      break;
    }
    case PIDS: // pids
    {
      if (redraw == 1)
        pids();
      break;
    }
    case GET_PARAMS: // get params
    {
      get_params();
      //EEPROM_writeParamCount(totalParams);
      break;
    }
    case EDIT_VIEW_PARAMS: // view/edit params
    {
      list_params();
      break; 
    }
    case INIT_EEPROM: // init eeprom
    {
      init_EEPROM();
      break;
    }
    case 10:
    {
      if (redraw == 1)
        edit_params();
      break;
    }
    case 11:
    {
      save_param();
      break; 
    }
  }
  if (millis() - hb_timer > 500)
  {
    hb_timer = millis();
    lcd.setCursor(19,3);
    if (beat == 1)
    {
      beat = 0;
      lcd.write(0);
    }
    else
      lcd_print_P(PSTR(" "));
  }
  gcs_update();
  Check_Buttons(4);
}

void main_menu() // menu 0
{
  lcd.clear();
  if (currentOption < 4)
  {
    lcd.setCursor(1,0);
    lcd_print_P(PSTR("Start Feeds"));
    lcd.setCursor(1,1);
    lcd_print_P(PSTR("Stop Feeds"));
    lcd.setCursor(1,2);
    lcd_print_P(PSTR("Flight Data"));
    lcd.setCursor(1,3);
    lcd_print_P(PSTR("PIDs"));
    lcd.setCursor(18,3);
    lcd.write(1);
  }
  if (currentOption > 3 && currentOption < 8)
  {
    lcd.setCursor(18,0);
    lcd.write(2);
    lcd.setCursor(1,0);
    lcd_print_P(PSTR("Get Params"));
    lcd.setCursor(1,1);
    lcd_print_P(PSTR("Edit/View Params"));
    lcd.setCursor(1,2);
    lcd_print_P(PSTR("Init EEPROM"));
    lcd.setCursor(1,3);
    lcd_print_P(PSTR("Menu 8"));
  }
  lcd.setCursor(0,currentOption%4);
  lcd_print_P(PSTR(">"));
  redraw = 0;  
}

void start_feeds()
{
  lcd.clear();
  lcd_print_P(PSTR("Starting feeds!"));
  mavlink_message_t msg;
  mavlink_msg_request_data_stream_pack(127, 0, &msg, 7, 1, MAV_DATA_STREAM_RAW_SENSORS, MAV_DATA_STREAM_RAW_SENSORS_RATE, MAV_DATA_STREAM_RAW_SENSORS_ACTIVE);
  send_message(&msg);

  mavlink_msg_request_data_stream_pack(127, 0, &msg, 7, 1, MAV_DATA_STREAM_EXTENDED_STATUS, MAV_DATA_STREAM_EXTENDED_STATUS_RATE, MAV_DATA_STREAM_EXTENDED_STATUS_ACTIVE);
  send_message(&msg);

  mavlink_msg_request_data_stream_pack(127, 0, &msg, 7, 1, MAV_DATA_STREAM_RAW_CONTROLLER, MAV_DATA_STREAM_RAW_CONTROLLER_RATE, MAV_DATA_STREAM_RAW_CONTROLLER_ACTIVE);
  send_message(&msg);

  mavlink_msg_request_data_stream_pack(127, 0, &msg, 7, 1, MAV_DATA_STREAM_POSITION, MAV_DATA_STREAM_POSITION_RATE, MAV_DATA_STREAM_POSITION_ACTIVE);
  send_message(&msg);

  mavlink_msg_request_data_stream_pack(127, 0, &msg, 7, 1, MAV_DATA_STREAM_EXTRA1, MAV_DATA_STREAM_EXTRA1_RATE, MAV_DATA_STREAM_EXTRA1_ACTIVE);
  send_message(&msg);

  mavlink_msg_request_data_stream_pack(127, 0, &msg, 7, 1, MAV_DATA_STREAM_RC_CHANNELS, MAV_DATA_STREAM_RC_CHANNELS_RATE, MAV_DATA_STREAM_RC_CHANNELS_ACTIVE);
  send_message(&msg); 
  delay(500);
  menu = MAIN_MENU;
  redraw = 1;
}

void stop_feeds()
{
  lcd.clear();
  lcd_print_P(PSTR("Stopping feeds!"));
  mavlink_message_t msg;
  mavlink_msg_request_data_stream_pack(127, 0, &msg, 7, 1, MAV_DATA_STREAM_ALL, 0, 0);
  send_message(&msg);
  delay(500);
  menu = MAIN_MENU;
  redraw = 1;
}

void flight_data() // menu 1
{
  if (millis() - timer > 200)
  {
    timer = millis();
    lcd.clear();
    switch (subMenu)
    {
      case 0:
      {
        lcd_print_P(PSTR("PT:"));
        lcd.print(pitch);
        lcd.setCursor(0,1);
        lcd_print_P(PSTR("RL:"));
        lcd.print(roll);
        lcd.setCursor(0,2);
        lcd_print_P(PSTR("YW:"));
        lcd.print(yaw);
        lcd.setCursor(0,3);
        lcd_print_P(PSTR("AT:"));
        lcd.print(altitude);
        lcd.setCursor(10,0);
        lcd_print_P(PSTR("VL:"));
        lcd.print(velocity);   
        lcd.setCursor(10,1);
        lcd_print_P(PSTR("LT:"));
        lcd.print(latitude);   
        lcd.setCursor(10,2);
        lcd_print_P(PSTR("LN:"));
        lcd.print(longitude);
        lcd.setCursor(10,3);
        lcd_print_P(PSTR("BT:"));
        lcd.print(battery/1000);
        lcd.setCursor(19,0);
        lcd_print_P(PSTR(">"));
        break;
      }
      case 1:
      {
        lcd.setCursor(0,0);
        lcd_print_P(PSTR("CH1:"));
        lcd.print(chan1);
        lcd.setCursor(0,1);
        lcd_print_P(PSTR("CH2:"));
        lcd.print(chan2);
        lcd.setCursor(0,2);
        lcd_print_P(PSTR("CH3:"));
        lcd.print(chan3);
        lcd.setCursor(0,3);
        lcd_print_P(PSTR("CH4:"));
        lcd.print(chan4);
        lcd.setCursor(10,0);
        lcd_print_P(PSTR("CH5:"));
        lcd.print(chan5);
        lcd.setCursor(10,1);
        lcd_print_P(PSTR("CH6:"));
        lcd.print(chan6);
        lcd.setCursor(10,2);
        lcd_print_P(PSTR("CH7:"));
        lcd.print(chan7);
        lcd.setCursor(10,3);
        lcd_print_P(PSTR("CH8:"));
        lcd.print(chan8); 
        lcd.setCursor(18,0);
        lcd_print_P(PSTR("<>"));
        break;        
      }
      case 2:
      {
        lcd.setCursor(0,0);
        lcd_print_P(PSTR("Mode:"));
        if (currentSMode == MAV_MODE_MANUAL)
          lcd_print_P(PSTR("ACRO"));
        else if (currentSMode == MAV_MODE_GUIDED)
          lcd_print_P(PSTR("STABALIZE"));
        else if (currentSMode == MAV_MODE_TEST1)
          lcd_print_P(PSTR("SIMPLE"));
        else if (currentSMode == MAV_MODE_TEST2)
          lcd_print_P(PSTR("ALT_HOLD"));
        else if (currentSMode == MAV_MODE_AUTO && currentNMode == MAV_NAV_HOLD)
          lcd_print_P(PSTR("LOITER"));
        else if (currentSMode == MAV_MODE_AUTO && currentNMode == MAV_NAV_WAYPOINT)
          lcd_print_P(PSTR("AUTO"));
        else if (currentSMode == MAV_MODE_AUTO && currentNMode == MAV_NAV_RETURNING)
          lcd_print_P(PSTR("RTL"));
        lcd.setCursor(19,0);
        lcd_print_P(PSTR("<"));
        break;
      }
    }

  }
}

void pids() // menu 2
{
  char currentMode[4][4] = {"ACR","STB","NAV","THR"};
  char currentAxis[3][4] = {"RLL","PIT","YAW"};
  char currentAxisNav[3][4] = {"LAT","LON","WP"};
  char currentAxisThr[2][4] = {"BAR","SON"};
  char pids[3][2] = {"P","I","D"};
  char currentPID[15];
  lcd.clear();
  lcd.setCursor(1,0);
  lcd.print(currentMode[subMenu][0]);
  if (subMenu == 1 || subMenu == 2 || subMenu == 3)
  {
    lcd.setCursor(0,0);
    lcd_print_P(PSTR("<"));
  }
  if (subMenu == 0 || subMenu == 1 || subMenu == 2)
  {
    lcd.setCursor(19,0);
    lcd_print_P(PSTR(">"));
  }
  for (int i=0; i<3; i++)
  {
    lcd.setCursor(0,i+1);
    lcd.print(pids[i]);
    lcd.setCursor(3 + (i*6),0);
    if (subMenu == 2)
      lcd.print(currentAxisNav[i]);
    else if (subMenu == 3)
    {
      if (i == 2)
        break;
      lcd.print(currentAxisThr[i]);
    }
    else
      lcd.print(currentAxis[i]);
    for (int j=0; j<3; j++)
    {
      strcpy(currentPID,currentMode[subMenu]);
      strcat(currentPID,"_");
      if (subMenu == 2)
        strcat(currentPID,currentAxisNav[i]);
      else if (subMenu == 3)
        strcat(currentPID,currentAxisThr[i]);
      else
        strcat(currentPID,currentAxis[i]);
      strcat(currentPID,"_");
      strcat(currentPID,pids[j]);
      int loc = find_param(currentPID);
      parameter temp;
      EEPROM_readParameter(loc * sizeof(temp), temp);
      if (menuMode == 1 && currentOption == ((i * 3) + j))
      {
        lcd.setCursor(1 + (i*6),j+1);
        lcd_print_P(PSTR(">"));
        strcpy(editParm.key, temp.key);
        editParm.value = temp.value;
      }
      else
        lcd.setCursor(2 + (i*6),j+1);
      lcd.print(temp.value);
    }
  }
  redraw = 0;
}

void get_params()
{
  if (paramsRecv >= TOTAL_PARAMS)
  {
    menu = MAIN_MENU;
    redraw = 1;
    return;
  }
  if (timeOut == GET_PARAMS_TIMEOUT)// || timeOut == 80 || timeOut == 60 || timeOut == 40 || timeOut == 20)  // request parameter every 2 seconds
  {
      mavlink_message_t msg;
      mavlink_msg_param_request_list_pack(127, 0, &msg, 7, 1);
      send_message(&msg);
      timeOut--; //just so we don't send twice during the same cycle...
  }
  if (millis() - timer > 200)
  {
    timer = millis();
    timeOut-=2;
    redraw = 1;
  }
  if (redraw == 1)
  {
    lcd.clear();
    lcd_print_P(PSTR("Requesting params..."));
    lcd.setCursor(0,1);
    lcd_print_P(PSTR("Params received:"));
    lcd.setCursor(6,2);
    lcd.print((int)paramsRecv);
    lcd_print_P(PSTR(" of "));
    lcd.print(TOTAL_PARAMS);
    redraw = 0;
  }
  if (timeOut < 1)
  {
    lcd.clear();
    lcd.setCursor(0,3);
    lcd_print_P(PSTR("Timed out!"));
    delay(1000);
    menu = MAIN_MENU;
    redraw = 1;
  }
}

void list_params()
{
  if (millis() - timer > 300)
  {
    timer = millis();
    timeOut--;
    redraw = 1;
    scroll++;
    if (scroll > 30000)
      scroll = 0;
  }
  if (redraw == 1)
  {
    lcd.clear();
    parameter temp;
    if (currentOption < 0)
      currentOption = TOTAL_PARAMS - 1;
    if (currentOption > TOTAL_PARAMS -1)
      currentOption = 0;
    for (int i=0; i<4; i++)
    {
      if (i + (currentOption/4 * 4) < TOTAL_PARAMS)
      {
        lcd.setCursor(0,i);
        EEPROM_readParameter((i + (currentOption/4 * 4)) * sizeof(temp), temp);
        if (strlen(temp.key)<10)
          lcd.print(temp.key);
        else
        {
          strcat(temp.key," ");
          for (int i=0; i<8; i++)
            lcd.print(temp.key[((i + (scroll % strlen(temp.key))) % strlen(temp.key))]);
          lcd.setCursor(8,i);
          lcd_print_P(PSTR(">"));
        }
        lcd.setCursor(10,i);
        if (i == currentOption%4)
        {
          lcd_print_P(PSTR(">"));
          strcpy(editParm.key, temp.key);
          editParm.value = temp.value;
        }
        else
          lcd_print_P(PSTR(" "));
        lcd.print(temp.value);
        if (i == currentOption%4)
          lcd_print_P(PSTR("<"));
      }
    }
    redraw = 0;
  }
}

void edit_params()
{
  lcd.clear();
  if (strlen(editParm.key)<10)
    lcd.print(editParm.key);
  else
  {
    for (int i=0; i<9; i++)
      lcd.print(editParm.key[i]);
  }
  lcd.setCursor(10,0);
  lcd_print_P(PSTR(">"));
  lcd.print(editParm.value);
  lcd_print_P(PSTR("<"));
  lcd.setCursor(0,1);
  lcd_print_P(PSTR("Up/Down -> +/- .01"));
  lcd.setCursor(0,2);
  lcd_print_P(PSTR("Right/Left -> +/- 1"));
  lcd.setCursor(0,3);
  lcd_print_P(PSTR("Back->Quit OK->Send"));
  redraw = 0;
}

void save_param()
{
  if ((timeOut == 100 || timeOut == 80 || timeOut == 60 || timeOut == 40 || timeOut == 20) && waitingAck == 1)  // request parameter every 2 seconds
  {
      mavlink_message_t msg;
      mavlink_msg_param_set_pack(127, 0, &msg, 7, 1, (int8_t *)editParm.key, editParm.value);
      send_message(&msg);
      timeOut--; //just so we don't send twice during the same cycle...
  }
  if (millis() - timer > 100)
  {
    timer = millis();
    timeOut--;
  }
  if (redraw == 1)
  {
    lcd.clear(); 
    lcd_print_P(PSTR("Sending new value..."));
    redraw = 0;
  }
  if (waitingAck == 0)
  {
    lcd.setCursor(0,2);
    lcd_print_P(PSTR("Value updated!"));
    delay(1000);
    if (menuMode == 0)
      menu = EDIT_VIEW_PARAMS;
    else if (menuMode == 1)
      menu = PIDS;
    redraw = 1;
  }
  if (timeOut < 1)
  {
    lcd.setCursor(0,2);
    lcd_print_P(PSTR("Timed out!"));
    delay(1000);
    if (menuMode == 0)
      menu = EDIT_VIEW_PARAMS;
    else if (menuMode == 1)
      menu = PIDS;
    redraw = 1;
    waitingAck = 0;
  }
}

int availableMemory() {
  int size = 2048;
  byte *buf;
  while ((buf = (byte *) malloc(--size)) == NULL)
    ;
  free(buf);
  return size;
}
